classdef KinemTraj % TODO: 转换为handle class
    %KINEMTRAJ 生成轨迹并进行导航仿真
    % Tips
    % # 地球坐标系选择EUGw，导航坐标系使用ENU地理坐标系g
    % # 输出的位置、速度、距离包含各杆臂点的输出，次序为V系原点、位移计杆臂点、惯组杆臂点
    %
    % Methods
    % # KinemTraj(samplePeriod, g, cst) 构造此类的实例
    % # obj = genTrajGenIn_veh(obj, genFuncName, genFuncInArg) 使用函数生成轨迹发生器输入量（载体轨迹）
    % # obj = genTrajGenIn_body(obj) 使用函数生成轨迹发生器输入量（IMU相对于载体轨迹）
    % # [out, auxOut, obj] = runTrajGen(obj) 运行轨迹发生器，输出运行结果
    %
    % Settable Properties
    % # In.vVRate, In.omegaNVV, In.omegaNVVRate, In.omegaVBB, In.tSects: 轨迹输入量
    %   In.nav_fB, In.nav_omegaIBB: 导航子系统的输入量，当cfg.enableTrajGen为false时使用这两个域，默认（不存在此域时，下同）为0
    % # Cfg.enableTrajGen: 逻辑标量，true表示使能轨迹生成子系统，默认为true
    %   Cfg.enableIMU: 逻辑标量，true表示使能惯组子系统，默认为true
    %   Cfg.enableDisplm: 逻辑标量，true表示使能位移计子系统，默认为false
    %   Cfg.enableNavigation: 逻辑标量，true表示使能导航子系统（用于验证轨迹生成子系统的输出），默认为false
    %   Cfg.forceRerun: 逻辑标量，true表示强制重新运行仿真（即使输入参数与上次相同且没有使能噪声），默认为false
    %   Cfg.IMU.enableErr: 逻辑标量，true表示使能惯组误差生成子系统，默认为true
    %   Cfg.IMU.enableNoise: 逻辑标量，true表示使能惯组噪声生成子系统，默认为false
    %   Cfg.IMU.ignore2OdErr: 逻辑标量，true表示忽略惯组误差模型中的二阶小量，默认为true
    %   Cfg.IMU.quantizeOut: 逻辑标量，true表示对惯组输出进行量化，使各周期的输出量为整数，默认为false
    %   Cfg.IMU.outFilePath: 字符串，指定惯组输出文件的路径，若为空则不生成惯组输出文件，默认为空
    %   Cfg.IMU.outPrecision: 字符串，惯组输出的精度，默认为'%.16e'
    %   Cfg.displm.enableErr: 逻辑标量，true表示使能位移计误差生成子系统，默认为true
    %   Cfg.displm.enableNoise: 逻辑标量，true表示使能位移计噪声生成子系统，默认为false
    %   Cfg.displm.quantizeOut: 逻辑标量，true表示对位移计输出进行量化，使各周期的输出量为整数，默认为false
    % # Par.samplePeriod: 标量，惯组输出的采样周期，单位s，默认为0.01
    %   Par.g: 标量，重力加速度，单位m/s^2，默认为9.8
    %   Par.CBVPeriod: 标量
    %   Par.IMU.acc: AccTriadPars类的对象
    %   Par.IMU.gyro: GyroTriadPars类的对象
    %   Par.IMU.lV: 3*1向量, 惯组相对于V系的杆臂参数，单位m，默认为0
    %   Par.displm.SF0: 3*1向量，位移计标度因数，单位(^/s)/m，默认为1
    %   Par.displm.dSF: 3*1向量，位移计标度因数误差，无单位，默认为0
    %   Par.displm.N: 3*1向量，位移计位移随机游走系数，单位m/sqrt(s)，默认为0
    %   Par.displm.lV: 标量，位移计相对于V系杆臂，默认为0
    % # InitCon.CVG: 3*3矩阵，G系相对于V系的初始姿态矩阵，默认为单位矩阵
    %   InitCon.CBV：3*3矩阵，V系相对于惯组坐标系的初始姿态矩阵，默认为单位矩阵
    %   InitCon.vG: 3*1向量，初始速度，单位m/s，默认为0
    %   InitCon.pos：3*1向量，分别为初始纬度、经度、高度，单位分别为rad、rad、m，默认为30°、110°、0m
    % # Cst.RE: 标量，地球长半轴，单位m，默认为6378137（CGCS2000模型值）
    %   Cst.omegaIE: 标量，地球自转角速率，单位rad/s，默认为7.292115e-05（CGCS2000模型值）
    %   Cst.eSq: 标量，地球偏心率的平方，无单位，默认为1-(1-1/2.98257222101e+02)^2（CGCS2000模型值）
    %   Cst.f: 标量，地球扁率，无单位，默认为1/2.98257222101e+02（CGCS2000模型值）
    %
    % Read-only Properties
    % # Out.t: n*1向量，轨迹时间，单位s
    %   Out.CVG: 3*3*n矩阵，n为采样数，V系轨迹姿态矩阵
    %   Out.CVL: 3*3*n矩阵，V系轨迹姿态矩阵（相对于L系）
    %   Out.vehicleAtt: n*3矩阵，V系轨迹姿态，分别为绕G系Z、Y、X顺序旋转得到V系的欧拉角，单位rad
    %   Out.CBG: 3*3*n矩阵，n为采样数，B系轨迹姿态矩阵
    %   Out.CBL: 3*3*n矩阵，B系轨迹姿态矩阵（相对于L系）
    %   Out.bodyAtt: n*3矩阵，B系轨迹姿态，分别为绕G系Z、Y、X顺序旋转得到B系的欧拉角，单位rad
    %   Out.vG: n*3*m矩阵，m为杆臂个数，杆臂端点速度，单位m/s
    %   Out.pos: n*3*m矩阵，杆臂端点位置，分别为纬度、经度、高度，单位分别为rad、rad、m
    %   Out.RE: n*3*m矩阵，E系下的杆臂端点位置坐标
    %   Out.s: n*m向量，杆臂端点里程，单位m
    %   Out.omegaNVV: n*3矩阵，V系下的V系相对于导航系的角速度，单位rad/s
    %   Out.omegaNBB: n*3矩阵，B系下的B系相对于导航系的角速度，单位rad/s
    %   Out.IMU.fB: n*3矩阵，加入惯组误差后的比力值，单位m/s^2
    %   Out.IMU.omegaIBB: n*3矩阵，加入惯组误差后的角速度值，单位rad/s
    %   Out.IMU.fBErrFree: n*3矩阵，轨迹比力值，单位m/s^2
    %   Out.IMU.omegaIBBErrFree: n*3矩阵，轨迹角速度值，单位rad/s
    %   Out.IMU.specVelInc: (n-1)*3矩阵，加入惯组误差后各采样周期内的比速度增量值，单位m/s
    %   Out.IMU.angleInc: (n-1)*3矩阵，加入惯组误差后各采样周期内的角增量值，单位rad
    %   Out.IMU.specVelIncErrFree: (n-1)*3矩阵，各采样周期内的比速度增量无误差值，单位m/s
    %   Out.IMU.angleIncErrFree: (n-1)*3矩阵，各采样周期内的角增量无误差值，单位rad
    %   Out.IMU.accOut: (n-1)*3矩阵，加速度计在各采样周期内的输出，单位^
    %   Out.IMU.gyroOut: (n-1)*3矩阵，陀螺在各采样周期内的输出，单位^
    %   Out.IMU.accOutErrFree: (n-1)*3矩阵，加速度计在各采样周期内的无误差输出，单位^
    %   Out.IMU.gyroOutErrFree: (n-1)*3矩阵，陀螺在各采样周期内的无误差输出，单位^
    %   Out.displm.vV: n*3矩阵，加入误差后的V系速度值，单位m/s
    %   Out.displm.vVErrFree: n*3矩阵，轨迹V系速度值，单位m/s
    %   Out.displm.displInc: (n-1)*3矩阵，加入误差后各采样周期内的位移增量值，单位m
    %   Out.displm.displIncErrFree: (n-1)*3矩阵，各采样周期内的位移增量无误差值，单位m
    %   Out.displm.displmOut: (n-1)*3矩阵，加入误差后位移计在各采样周期内的位移增量输出，单位^
    %   Out.displm.displmOutErrFree: (n-1)*3矩阵，位移计在各采样周期内的无误差位移增量输出，单位^
    % # AuxOut.nav.CBG: 3*3*n矩阵，导航子系统输出的姿态矩阵
    %   AuxOut.nav.att: n*3矩阵，导航子系统输出的姿态
    %   AuxOut.nav.vG: n*3矩阵，导航子系统输出的速度
    %   AuxOut.nav.pos: n*3矩阵，导航子系统输出的位置
    %   AuxOut.TGI.vVRate, AuxOut.TGI.omegaNVV: 轨迹输入量
    %   AuxOut.TGI.tSects: 各轨迹段的分隔时间点，单位s
    %
    % References
    % # 《应用导航算法工程基础》“轨迹发生器”
    
    properties (AbortSet)
        In(1, 1) struct = struct('vVRate', NaN(1, 3), 'omegaNVV', NaN(1, 3), 'omegaNVVRate', NaN(1, 3), ...
            'omegaVBB', struct('time', 0, 'signals', struct('values', zeros(1, 3), 'dimensions', 3)), 'tSects', [], ...
            'nav_fB', struct('time', 0, 'signals', struct('values', zeros(1, 3), 'dimensions', 3)), ...
            'nav_omegaIBB', struct('time', 0, 'signals', struct('values', zeros(1, 3), 'dimensions', 3)))
        Cfg(1, 1) struct = struct('enableTrajGen', true, 'enableNavigation', false, ...
            'enableIMU', true, 'enableDisplm', false, 'forceRerun', false, ...
            'IMU', struct('enableErr', true, 'enableNoise', false, 'ignore2OdErr', true, 'quantizeOut', false, 'outFilePath', '', 'outPrecision', '%.16e'), ...
            'displm', struct('enableErr', true, 'enableNoise', false, 'quantizeOut', false))
        Par(1, 1) struct = struct('samplePeriod', 0.01, 'g', 9.8, 'CBVPeriod', NaN, ...
            'IMU', struct('acc', AccTriadPars, 'gyro', GyroTriadPars, 'lV', zeros(3, 1)), ...
            'displm', struct('SF0', ones(3, 1), 'dSF', zeros(3, 1), 'N', zeros(3, 1), 'lV', zeros(3, 1)))
        InitCon(1, 1) struct = struct('CVG', eye(3), 'CBV', eye(3), 'vG', zeros(3, 1), 'pos', [30/180*pi 110/180*pi 0]')
        Cst(1, 1) struct = struct('RE', 6378137, 'omegaIE', 7.292115e-05, 'eSq', 1-(1-1/2.98257222101e+02)^2, 'f', 1/2.98257222101e+02)
    end
    
    properties (SetAccess = private)
        Out(1, 1) struct
        AuxOut(1, 1) struct
    end
    
    properties (Access = private, Transient)
        PubPropsAreFresh(1, 1) logical = false
    end
    
    properties (Access = private, Constant)
        Cst_CNL = [0 1 0; 1 0 0; 0 0 -1]
    end
    
    methods
        function obj = KinemTraj(samplePeriod, g, cst)
            %KINEMTRAJ 构造此类的实例
            % Input Arguments
            % samplePeriod: 参见Par.samplePeriod说明
            % g: 参见Par.g说明
            % cst: 参见Cst说明
            
            if nargin > 2
                obj.Cst = structcpy_specfields(obj.Cst, cst, {'RE', 'omegaIE', 'eSq', 'f'});
            end
            if nargin > 1
                obj.Par.g = g;
            end
            if nargin > 0
                obj.Par.samplePeriod = samplePeriod;
            end
        end
        
        function obj = genTrajGenIn_veh(obj, genFuncName, genFuncInArg)
            % Input Arguments
            % genFuncName: 字符串，轨迹输入量生成函数名称
            % genFuncInArg: 元胞向量，轨迹输入量生成函数参数
            
            tmp = [genFuncName '('];
            for i=1:length(genFuncInArg)
                if i > 1
                    tmp = [tmp ', ']; %#ok<AGROW>
                end
                tmp = [tmp 'genFuncInArg{' int2str(i) '}']; %#ok<AGROW>
            end
            tmp = [tmp ')'];
            [obj.In.vVRate, obj.In.omegaNVV, obj.In.omegaNVVRate, obj.In.tSects] = eval(tmp); % NOTE: tSects中应包含轨迹起始时刻及结束时刻
        end
        
        function obj = genTrajGenIn_body(obj)
            % 以下omegaVBB仅作为示例，实际仿真时根据需要进行改动
            obj.In.omegaVBB.time = obj.In.omegaNVV.time;
            obj.In.omegaVBB.signals.dimensions = 3;
            obj.In.omegaVBB.signals.values = zeros(length(obj.In.omegaNVV.time), 3);
            symbol = 1;
            dt = obj.In.omegaVBB.time(2) - obj.In.omegaVBB.time(1);
            for i=1:length(obj.In.omegaVBB.time)
                if mod(obj.In.omegaVBB.time(i), obj.Par.CBVPeriod) == 0 && obj.In.omegaVBB.time(i) ~= 0
                    [ ~, sectomegaVBBValues, ~, ~ ] = geneletgi_rotabtbdfixaxis(obj.In.omegaVBB.time(i), symbol * [0 0 pi]', pi/10, 1, 1, dt); % 利用现有函数
                    obj.In.omegaVBB.signals.values(i:i+length(sectomegaVBBValues)-1,:) = sectomegaVBBValues;
                    symbol = -symbol; % 正转、反转结合
                end
            end
            obj.In.omegaVBB.signals.values = obj.In.omegaVBB.signals.values(1:length(obj.In.omegaVBB.time),:);
        end
        
        function [out, auxOut, obj] = runTrajGen(obj)
            
            needRerun = obj.PubPropsAreFresh || obj.Cfg.forceRerun || obj.Cfg.IMU.enableNoise || obj.Cfg.displm.enableNoise;
            if ~needRerun
                out = obj.Out;
                auxOut = obj.AuxOut;
                disp('输入参数与上次调用相同且未使能噪声，直接输出上次轨迹');
                return;
            end
            
            [in, cfg, par, iniCon, cst] = deal(obj.In, obj.Cfg, obj.Par, obj.InitCon, obj.Cst); % TODO: 修改mdl文件以增加obj变量名，可省略此步骤
            
            % 调整分段参数尺寸
            nSects = length(obj.In.tSects) - 1;
            par.tSects = obj.In.tSects;
            par.IMU.acc = struct(resizeFieldLastDim(obj.Par.IMU.acc, nSects));
            par.IMU.acc.noise = structcpy_specfields(struct, par.IMU.acc, {'R', 'K', 'B', 'N', 'Q', 'qc', 'Tc', 'Omega0', 'f0'});
            par.IMU.gyro = struct(resizeFieldLastDim(obj.Par.IMU.gyro, nSects));
            par.IMU.gyro.noise = structcpy_specfields(struct, par.IMU.gyro, {'R', 'K', 'B', 'N', 'Q', 'qc', 'Tc', 'Omega0', 'f0'});
            par.displm.noise.N = par.displm.N;
            par.lV = [zeros(3, 1) obj.Par.displm.lV obj.Par.IMU.lV]; % 第一列表示V系原点，从第2列开始依次为各传感器杆臂参数（目前包括位移计、惯组）
            
            % 初始化
            n = size(par.lV, 2);
            iniCon.pos = repmat(obj.InitCon.pos, 1, n);
            for i = 1:n
                iniCon.pos(:, i) = levarmcomp_pos(iniCon.pos(:, i), obj.InitCon.CVG, -par.lV(:, i), obj.Cst.f, obj.Cst.RE);
            end
            
            % 运行仿真
            simOutputTimes = (0:obj.Par.samplePeriod:obj.In.tSects(end));
            simOptions = simset('DstWorkspace', 'current', 'SrcWorkspace', 'current');
            sim('gentrajectorymdl', simOutputTimes, simOptions);
            disp('完成轨迹模型运行');
            
            % 轨迹生成器输出量
            out.t = simOutputTimes';
            out.CVG = trajGen_CVN;
            out.CVL = NaN(size(out.CVG));
            for i=1:size(out.CVG, 3)
                out.CVL(:, :, i) = obj.Cst_CNL * out.CVG(:, :, i);
            end
            out.vehicleAtt = trajGen_vehicleAtt;
            out.CBG = trajGen_CBN;
            out.CBL = NaN(size(out.CBG));
            for i=1:size(out.CBG, 3)
                out.CBL(:, :, i) = obj.Cst_CNL * out.CBG(:, :, i);
            end
            out.bodyAtt = trajGen_bodyAtt;
            if size(par.lV, 2) > 1
                out.fV =  permute(trajGen_fV, [3 1 2]);
                out.vG = permute(trajGen_vN, [3 1 2]);
                out.pos = permute(vertcat(trajGen_L, trajGen_lambda, trajGen_h), [3 1 2]);
                out.RE = permute(trajGen_RE, [3 1 2]);
                out.s = squeeze(trajGen_s)';
            else
                out.fV =  trajGen_fV;
                out.vG = trajGen_vN;
                out.pos = horzcat(trajGen_L, trajGen_lambda, trajGen_h);
                out.RE = trajGen_RE;
                out.s = trajGen_s;
            end
            out.omegaNVV = trajGen_omegaNVV;
            out.omegaIVV = trajGen_omegaIVV;
            out.omegaENN = trajGen_omegaENN;
            out.vNRate = trajGen_vNRate;
            
            % IMU输出量
            out.IMU = genimuout(IMU_fB, IMU_omegaIBB, IMU_fBErrFree, IMU_omegaIBBErrFree, ...
                IMU_fBInt, IMU_omegaIBBInt, IMU_fBIntErrFree, IMU_omegaIBBIntErrFree, cfg.IMU, par.IMU, par.samplePeriod, par.tSects, nSects);
            
            % 位移计输出量
            out.displm = gendisplmout(displm_vV, displm_vVErrFree, displm_vVInt, displm_vVIntErrFree, cfg.displm, par.displm);
            disp('完成轨迹输出量生成');
            
            % 辅助输出量
            auxOut.nav.CBG = nav_CBN;
            auxOut.nav.att = nav_att;
            auxOut.nav.vG = nav_vN;
            auxOut.nav.pos = horzcat(nav_L, nav_lambda, nav_h);
            if isfield(in, 'genFuncName') && ~isempty(in.genFuncName)
                auxOut.TGI.vVRate = in.vVRate;
                auxOut.TGI.omegaNVV = in.omegaNVV;
                auxOut.TGI.omegaNVVRate = in.omegaNVVRate;
            end
            auxOut.TGI.tSects = par.tSects;
            obj.Out = out;
            obj.AuxOut = auxOut;
            obj.PubPropsAreFresh = false;
        end
        
        function obj = set.In(obj, in)
            obj.In = in;
            obj.PubPropsAreFresh = true;
        end
        
        function obj = set.Cfg(obj, cfg)
            obj.Cfg = cfg;
            obj.PubPropsAreFresh = true;
        end
        
        function obj = set.Par(obj, par)
            obj.Par = par;
            obj.PubPropsAreFresh = true;
        end
        
        function obj = set.InitCon(obj, initCon)
            obj.InitCon = initCon;
            obj.PubPropsAreFresh = true;
        end
        
        function obj = set.Cst(obj, cst)
            obj.Cst = cst;
            obj.PubPropsAreFresh = true;
        end
    end
end